/*!
  ******************************************************************************
  * @file           : main.c
  * @author         :He Kunling
  * @createDate     : 2021/07/31
  * @lastEditors    :
  * @lastEditTime   : 2021/07/31
  * @brief          : 
  * 
  *     This file provides functions bellow to be called:
  * 
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2021 Zero_One NUAA.
  * All rights reserved.
  *
  ******************************************************************************
  */
 /* Includes ------------------------------------------------------------------*/
#include "main.hpp"

using namespace std;


int main(int argc, char** argv)
{
  target_latlng first_latlng;
  first_latlng.target_lat = 30.0001;//纬度
  first_latlng.target_lng = 130.0001;//经度（对应仿真x）
  target_latlng_t test = according_distance_caculate_latlng(1.344,0,first_latlng.target_lat, first_latlng.target_lng);
  target_pos_t caculate_pos = get_vector_xy_from_current_position_NE_double(first_latlng.target_lng,test.target_lng,
                                                                              first_latlng.target_lat, test.target_lat);//通过rtk计算距离误差  
  printf("x offset :%f, y offset:%f", caculate_pos.target_pos_x, caculate_pos.target_pos_y);//测试经纬度与距离的转换
  //   /* -------------------- initialization ---------------------------------------- */   
    // Initialize variables
    file = open("/dev/i2c-8",O_RDWR);
    ioctl(file,I2C_SLAVE,TF_mini_ADD);
    IARC_task_init();

    usr_para_init();
    //初始化UDP
    UDP_fd = UDP_init();

   // Setup Usr Serial
    const char* radio_dev = "/dev/radio";
    unsigned int radio_baudrate = 57600;
    LinuxSerialDevice radio(radio_dev,radio_baudrate);
    // NX_serial_init(&radio);

    const char* flightcontrol_dev = "/dev/ttyUSB0";
    unsigned int flightcontrol_baudrate = 115200;
    LinuxSerialDevice flightcontrol(flightcontrol_dev,flightcontrol_baudrate);
    NX_serial_init(&flightcontrol);

    const char* camera_dev = "/dev/camera";
    unsigned int camera_baudrate = 115200;
    LinuxSerialDevice camera(camera_dev,camera_baudrate);
    // NX_serial_init(&camera);

    const char* SBUS_dev = "/dev/ttyTHS0";
    // const char* SBUS_dev = "/dev/ttyAMA0";
    unsigned int SBUS_baudrate = 115200;
    LinuxSerialDevice SBUS_read(SBUS_dev,SBUS_baudrate);
    NX_serial_init(&SBUS_read);

    
    sleep(1);
 /* -------------------- create threads ---------------------------------------- */
    usr_thread_t* usr_thread_para;
    pthread_t id_telemetry = 3;
    pthread_t id_control = 4;
    pthread_t id_camera = 5;
    pthread_t id_SBUS = 6;
    int is_pthread_created;
    // Create Control Thread
    usr_thread_para =  (usr_thread_t*)malloc(sizeof(usr_thread_t));
    usr_thread_para -> usr_radio = &radio;
    usr_thread_para -> usr_flightcontrol = &flightcontrol;
    usr_thread_para -> usr_camera = &camera;
    usr_thread_para-> usr_SBUS = &SBUS_read;

      // Create Control Thread
    is_pthread_created = pthread_create(&id_telemetry, NULL, telemetry_thread, (void*)usr_thread_para);
    is_pthread_created = pthread_create(&id_control, NULL, control_thread, (void*)usr_thread_para);
    is_pthread_created = pthread_create(&id_camera, NULL, camera_thread, (void*)usr_thread_para);
    is_pthread_created = pthread_create(&id_SBUS, NULL, SBUS_thread, (void*)usr_thread_para);
    sleep(2);

    char input_char;
    uint64_t time_5ms = 0; 
    uint8_t loop_1Hz  = 0;

    while(1){




  }
          UDP_close(UDP_fd);
              std::cout << " thread exit successfully!" << std::endl;
     return 0;
}
